#!/usr/bin/env python
# license removed for brevity

import rospy
import sys
import actionlib
from tf.transformations import euler_from_quaternion,quaternion_from_euler
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
roll=pitch=yaw=0.0
def movebase_client():
    global roll,pitch,yaw
    client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
    client.wait_for_server()

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = float(sys.argv[1])
    goal.target_pose.pose.position.y = float(sys.argv[2])
    yaw=float(sys.argv[3])
    quaternion=quaternion_from_euler(0.0,0.0,yaw)
    goal.target_pose.pose.orientation.z= quaternion[2]
    goal.target_pose.pose.orientation.w= quaternion[3]

    client.send_goal(goal)
    wait = client.wait_for_result()
    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        return client.get_result()

if __name__ == '__main__':
    try:
        rospy.init_node('movebase_client_py')
        result = movebase_client()
        if result:
            rospy.loginfo("Goal execution done!")
    except rospy.ROSInterruptException:
        rospy.loginfo("Navigation test finished.")

